%hai
%% UAV VTP trim
% In this file we try to trim the VTP aircraft model
% For information about setting and using variables, c.f trim_tutorial.m in
% the UMN_UAV_simulation
%
% Definition of input and states vector
%  Deflection of control surfaces in deg
%       u(1:5) =  [aileron(da) elevator(de) rudder(dr) flap(df)
%       throttle(dT)]
%  Initial states values
%       x(1:13) = [xe ye ze Vu Vv Vw phi theta psi p q r ...hdot] 
%       -- notes: hdot need to be reconsidered
%
%
clc;
clear all;
% required to set some time-derivatives to zero when trimming the aircraft
xfix = 1;
% Simulation sample time
SampleTime = 0.02; % sec
%%%% fixstates
data_aero_vtp;
disp('Trimming process for VT Patrol');
% make the name of the aircraft
sysname = 'VTP_airframe';
opt = menu('Select type of steady-state flight:',...
    'Steady wings-level flight','Steady tunning flight',...
    'Steady pull-up','Steady roll','Quit');
if opt == 1
    % STEADY WING LEVEL FLIGHT
    %--------------------------
    clc;
    disp('Steady wings-level flight.');
    disp('==========================');
    
    V = input('Give desired airspeed [m/s], default = 30: ');
    if isempty(V)
        V = 30;
    end
    
    H = input('Give (initial) altitude [m], default = 300: ');
    if isempty(H)
        H=300;
    end
    
    psi = input('Give heading [deg], default = 0: ')*pi/180;
    if isempty(H)
        psi = 0;
    end
    phidot   = 0;
    psidot   = 0;
    thetadot = 0;
    
elseif opt == 2
    clc;
    disp('Steady turning flight.');
    disp('======================');
    
    V = input('Give desired airspeed [m/s], default = 30: ');
    if isempty(V)
        V = 30;
    end
    
    H = input('Give initial altitude [m], default = 300: ');
    if isempty(H)
        H = 300;
    end
    
    psi = input('Give initial heading [deg], default = 0: ')*pi/180;
    if isempty(psi)
        psi = 0;
    end
    
    phidot = 0;
    thetadot = 0;
    
    answ = input('Specify the turnrate (1) or turnradius (2)? ');
    if answ == 1
        psidot = input('Give desired rate of turn [deg/s], default = 0: ')*pi/180;
        if isempty(psidot)
            psidot = 0;
        end
    elseif answ == 2
        R = input('Give desired turn radius (>0) [m], default = straight & level: ');
        if isempty(R) ~= 1
            psidot = V/R;
        else
            psidot = 0;
        end
    end
    clear answ
    
elseif opt == 3
    % STEADY PULL-UP
    % --------------
    clc;
    disp('Steady pull-up.');
    disp('===============');
    
    V = input('Give desired airspeed [m/s], default = 30: ');
    if isempty(V)
        V = 30;
    end
    
    H = input('Give initial altitude [m], default = 300: ');
    if isempty(H)
        H = 300;
    end
    
    psi = input('Give initial heading [deg], default = 0: ')*pi/180;
    if isempty(psi)
        psi = 0;
    end
    phidot = 0;
    psidot = 0;
    
    thetadot = input('Give pull-up rate [deg/s], default = 0: ')*pi/180;
   if isempty(thetadot)
      thetadot = 0;
   end

   rolltype = 's';   % No rolling, so default setting, which is stability-
                     % axes roll, will be used.

elseif opt == 4
    % Steady roll
    %------------
    clc;
    disp('Steady roll.');
    disp('============');
    
    V = input('Give desired airspeed [m/s], default = 30: ');
    if isempty(V)
        V=30;
    end
    
    H = input('Give initial altitude [m], default = 300: ');
    if isempty(H)
        H = 300;
    end
    
    psi = input('Give initial heading [deg], default = 0 ')*pi/180;
    if isempty(psi)
        psi = 0;
    end
    
    thetadot = 0;
    psidot   = 0;
    
    phidot = input('Give desired roll-rate [deg.s-1], default = 0: ')*pi/180;
    if isempty(phidot)
        phidot = 0;
    end

else
    % set help variable skip = 1, to ensure that the aircraft configuration
    % does not have to be entered if the user chooses option 5 to QUIT
    % --------------------------------------------------------------------
    skip = 1;
end
  
SampleTime = 0.02;
%% Set Aircraft Initial Conditions
% %  Deflection angle for control surfaces are in deg
TrimCondition.Inputs.da = 0;       %deg
TrimCondition.Inputs.de = -1.1;          %deg
TrimCondition.Inputs.dr = 0;          %deg
TrimCondition.Inputs.df = 0;          %deg
TrimCondition.Inputs.dT = 0.4;        %nd,0 to 1

% % Initial state values
TrimCondition.X0     = Xme_0;    %initial NED position
TrimCondition.Vb0    = Uvw0;     %initial body velocity
TrimCondition.Euler0 = euler_0;  %initial euler orientation (roll,pitch,yaw)
TrimCondition.pqr0   = pqr0;     %initial body frame rate
% 
MaxLimControl = 40;
MinLimControl = -40;
% 
% %% Create operating point specification for the sim
op_spec  = operspec('VTP_airframe');
% 
% %% Specifying State Trim Conditions
get(op_spec.States);
% 
% % Euler angle (phi theta psi)
op_spec.States(1).Known       = [0 ; 0; 1];
op_spec.States(1).x= TrimCondition.Euler0;
op_spec.States(1).SteadyState = [0; 0; 0];
% 
% % Angular rates p,q,r
op_spec.States(2).Known       = [0; 0; 0];
op_spec.States(2).x           = TrimCondition.pqr0;
op_spec.States(2).SteadyState = [1; 1; 1];
% 
% % body frame velocity ub,vb,wb
op_spec.States(3).Known       = [1;0;0];
op_spec.States(3).x           = TrimCondition.Vb0;
op_spec.States(3).SteadyState = [1; 1; 1];
op_spec.States(3).min         = [0 -inf -inf];
% 
% % earth position (north east down) (xE,yE,zE)
op_spec.States(4).Known       = [0; 0; 1];
op_spec.States(4).x           = TrimCondition.X0;
op_spec.States(4).SteadyState = [0; 0; 1];
op_spec.States(4).Max         = [inf inf 0];
% 
% %% set input constraints
% 
% % aileron deflection
op_spec.Inputs(1).Known = 1;
op_spec.Inputs(1).u     = TrimCondition.Inputs.da;
op_spec.Inputs(1).Max   = MaxLimControl;
op_spec.Inputs(1).Min   = MinLimControl;
% 
% % elevator deflection
op_spec.Inputs(2).Known = 0;
op_spec.Inputs(2).u     = TrimCondition.Inputs.de;
op_spec.Inputs(2).Max   = MaxLimControl;
op_spec.Inputs(2).Min   = MinLimControl;
% 
% % rudder deflection
op_spec.Inputs(3).Known = 1;
op_spec.Inputs(3).u     = TrimCondition.Inputs.dr;
op_spec.Inputs(3).Max   = MaxLimControl;
op_spec.Inputs(3).Min   = MinLimControl;
% 
% % flap deflection
op_spec.Inputs(4).Known = 1;
op_spec.Inputs(4).u     = TrimCondition.Inputs.df;
op_spec.Inputs(4).Max   = MaxLimControl;
op_spec.Inputs(4).Min   = MinLimControl;
% 
% % throttle control
op_spec.Inputs(5).Known = 1;
op_spec.Inputs(5).u     = TrimCondition.Inputs.dT;
op_spec.Inputs(5).Max   = 1;
op_spec.Inputs(5).Min   = 0;
% 
%% Trimming the model
[op_trim, op_report] = findop('VTP_airframe',op_spec);
% 
% %%
% % sys_io(1) = linio('VTP_airframe_long_trim/de',1,'in');
% % sys_io(2) = linio('VTP_airframe_long_trim/theta',1,'out');
% linsys = linearize('VTP_airframe_long_trim',op_point);
% 
% set(linsys,'OutputName',{'x';'y';'z';'\phi';'\theta';'\psi';'Vx';'Vy';...
%     'Vz';'p';'q';'r'});
% set(linsys,'StateName',{'\phi';'\theta';'\psi';'p';'q';'r';'ub';'vb';...
%     'wb';'xe';'ye';'ze'});
%  
% 

